import socket
import re
import time
from datetime import datetime
from threading import Thread


# 雷达通信主线程
class radar_client(Thread):
    def __init__(self):
        Thread.__init__(self)
        self.runing = True
        self.s = socket.socket()  # Create a socket object
        # s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        # host = socket.gethostname()  # Get local machine name
        host = '192.168.4.123'  # Get local machine name
        # host = 'localhost'
        client_port = 49157  # Reserve a port for your service.
        server_port = 60000    # 雷达方的端口号
        # s.bind((host, client_port))  # Bind to the port
        try:
            self.s.connect((host, server_port))
            print("Connected Successfully!")
        except Exception as e:
            print("something's wrong with %s:%d. Exception is %s" % (host, server_port, e))
        self.current = 0.0
        self.error_list = []
        
    def run(self):
        # 雷达启动运行指令
        radar_steart = b'\xa5\x5a\x10\xb5\x03\x01\x00\x00\x00\xa0\x69'
        radar_steart_flag = self.s.send(radar_steart)

        if (radar_steart_flag != None):
            print("通信连接")
            radar_respond = self.s.recv(12)
            print("应答指令：", radar_respond.hex())

            flag = self.s.recv(5)
            print("误差测试:", flag)

            sync = self.s.recv(1)
            system_time0 = str(datetime.now())  # Ts1
            print("\n收到第一次报文：", sync)
            radar_data0 = self.s.recv(26)
            radar_time0 = radar_data0.decode("utf-8")  # Tm1
            print("雷达端第一次时间戳：", radar_time0)
            print("激光端第一次时间戳：", system_time0)
            time.sleep(0.500)  # 延时500ms后进行第二次同步

            self.s.send(b'2')
            system_time1 = str(datetime.now())  # Ts2
            radar_data1 = self.s.recv(26)
            radar_time1 = radar_data1.decode("utf-8")  # Tm2
            print("\n发送第二次报文：", b'2')
            print("激光端第二次时间戳：", system_time1)
            print("雷达器端第二次时间戳：", radar_time1)

            rad_time0 = re.split(':', radar_time0)
            rad0 = float(rad_time0[-1])
            sys_time0 = re.split(':', system_time0)
            sys0 = float(sys_time0[-1])
            rad_time1 = re.split(':', radar_time1)
            rad1 = float(rad_time1[-1])
            sys_time1 = re.split(':', system_time1)
            sys1 = float(sys_time1[-1])
            delay0 = (sys0 - rad0) * 1000
            delay1 = (rad1 - sys1) * 1000
            aver_delay = (delay0 + delay1) / 2
            # print("两端的通信延迟为：", aver_delay)
            offset = (delay0 - delay1) / 2
            print("\n激光端与雷达端的系统时间偏差为：", offset)

    def stop(self):
        self.runing = False

    def close(self):
        self.s.close()


if __name__ == '__main__':  # 主函数入口
    radar = radar_client()  # 定义一个radar客户端
    radar.start()
